import svar
import numpy as np
import cv2
import time
import json
import requests
import os
import shutil

rtmv=svar.load('svar_rtmv')
cache_dir='/tmp/image_cache/'
if os.path.exists(cache_dir):
    shutil.rmtree(cache_dir)
os.makedirs(cache_dir)
options={"ip":"192.168.1.183","port":1025,"skip":1}

dataset=rtmv.DatasetNetwork(options)
last_time=0
latest_fr=None

def post_liveframe(url,imagefile,jsonfile):
    f_json=open(jsonfile,'rb')
    f_img=open(imagefile, "rb")
    files={
        jsonfile : f_json,imagefile : f_img
       }
    try:
        r = requests.post(url, None, files=files)
        print(r.text)
    except:
        f_json.close()
        f_img.close()
        print("failed to post",imagefile)

def callback_frame(fr):
  if fr == None:
    return

  global latest_fr
  latest_fr=fr

  
  global last_time
  if fr.timestamp() < last_time+1:
    return
  last_time=fr.timestamp()

  lla=fr.getGPSLLA()

  image=fr.getImage(0,0)
  
  m= np.array(image)
  m.dtype='uint8'

  #cv2.imshow('video', m)
  #cv2.waitKey(5)


  # meta
  rad2degree=180/3.1415926
  gps={"longitude":lla["lng"]*rad2degree,"latitude":lla["lat"]*rad2degree,"altitude":lla["alt"]}
  gpsSigma={"longitude":5,"latitude":5,"altitude":10}
  attitudeSigma={"pitch":1,"roll":1,"yaw":10}
  h=fr.getHeight2Ground()
  height={"sigma":h["sigma"],"value":h["height"]}
  frame_json={"timestamp":fr.timestamp(),"gps":gps,"gpsSigma":gpsSigma,"height":height,
              #"attitude":fr.getPitchYawRoll(),"attitudeSigma":attitudeSigma,
              "camera":"h20t_rtmv"}

  print(frame_json)
  image_file=os.path.join(cache_dir,str(fr.timestamp())+".jpg")
  json_file=os.path.join(cache_dir,str(fr.timestamp())+".json")
  cv2.imwrite(image_file,m)
  open(json_file,"w").write(json.dumps(frame_json))

  print(m.shape,m.dtype.name) # BGRA
  post_liveframe('http://192.168.1.232:1024/liveframe',json_file,image_file)

sub_kf= rtmv.messenger.subscribe('UI.output.curframe',0,callback_frame)


#while True:
#  time.sleep(1)

while True:
  fr=latest_fr
  if fr == None:
    continue

  #print(fr.id(),fr.timestamp(),fr.getImage(0,0),fr.getHeight2Ground(),fr.getGPSLLA(),fr.getPitchYawRoll())

  image=fr.getImage(0,0)
  
  m= np.array(image)
  m.dtype='uint8'

  cv2.imshow('video', m)
  cv2.waitKey(5)
  time.sleep(0.1)


